#include "path_plan/trajectory_points.hpp"

class CombinedNode : public rclcpp::Node
{
public:
    CombinedNode() : Node("combined_node")
    { // 初始化 CombinedNodeMembers 内部成员
        // 初始化力数据订阅
        members_.force_sub = this->create_subscription<geometry_msgs::msg::TwistStamped>(
            "/force_data", 10,
            std::bind(&CombinedNode::force_callback, this, std::placeholders::_1));

        // 初始化 result2_topic 订阅者
        members_.result_sub = this->create_subscription<std_msgs::msg::Int32>(
            "result2_topic", 10,
            std::bind(&CombinedNode::result_callback, this, std::placeholders::_1));

        // 初始化 MoveGroupInterface
        members_.move_group = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
            std::shared_ptr<rclcpp::Node>(this), "zc4");

        RCLCPP_INFO(get_logger(), "节点初始化完成");
    }

    std::shared_ptr<VisualControl> get_visual_control_node()
    {
        return members_.visual_control_node;
    }

    void start_execution(int path_index)
    {
        if (members_.is_executing.exchange(true))
        {
            RCLCPP_WARN(get_logger(), "路径执行已在进行中，忽略新的请求");
            return;
        }
        RCLCPP_INFO(get_logger(), "开始执行路径 %d", path_index);
        std::thread([this, path_index]()
                    {
            bool path_execution_successful = false;
            try {
                switch (path_index) {
                    case 3:
                        executeCartesianPath3();
                        path_execution_successful = true;
                        break;
                }
                if (path_execution_successful && path_index == 3) {
                    RCLCPP_INFO(get_logger(), "路径 %d 执行完成，开始执行后续移动和力监控序列...", path_index);
                    handlePostPathSequence();
                } else if (path_execution_successful) {
                    RCLCPP_INFO(get_logger(), "路径 %d 执行完成。", path_index);
                    members_.is_executing = false;
                } else {
                    RCLCPP_WARN(get_logger(), "路径 %d 执行未标记为成功，不执行后续序列。", path_index);
                    members_.is_executing = false;
                }
            } catch (const std::exception& e) {
                RCLCPP_ERROR(get_logger(), "路径执行线程中发生异常: %s", e.what());
                members_.is_executing = false;
            } catch (...) {
                RCLCPP_ERROR(get_logger(), "路径执行线程中发生未知异常");
                members_.is_executing = false;
            } })
            .detach();
    }

private:
    path_plan::CombinedNodeMembers members_;

    bool isArmAtMiddle() const
    {
        auto middle = path_plan::getMiddlePose();
        geometry_msgs::msg::Pose current_pose = members_.move_group->getCurrentPose().pose;
        double dx = current_pose.position.x - middle.position.x;
        double dy = current_pose.position.y - middle.position.y;
        double dz = current_pose.position.z - middle.position.z;
        double distance = std::sqrt(dx * dx + dy * dy + dz * dz);
        const double tol = 0.5;
        return (distance < tol);
    }

    void force_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg)
    {
        members_.current_z_force.store(msg->twist.linear.z);
        if (std::abs(msg->twist.linear.z) >= 5.0)
            members_.force_sign = false;
        else
            members_.force_sign = true;
    }

    void result_callback(const std_msgs::msg::Int32::SharedPtr msg)
    {
        if (msg->data == 1)
        {
            if (isArmAtMiddle())
            {
                RCLCPP_INFO(get_logger(), "机械臂处于 middle 位置，result2_topic 值为 1，开始执行路径 3");
                start_execution(3);
            }
            else
            {
                RCLCPP_WARN(get_logger(), "机械臂不在 middle 位置，忽略执行路径3请求");
            }
        }
    }

    bool moveToNamedTarget(const std::string &target_name)
    {
        RCLCPP_INFO(get_logger(), "尝试移动到命名目标: %s", target_name.c_str());
        members_.move_group->setNamedTarget(target_name);
        moveit::planning_interface::MoveGroupInterface::Plan plan;
        bool success = (members_.move_group->plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
        if (success)
        {
            if (members_.move_group->execute(plan) == moveit::core::MoveItErrorCode::SUCCESS)
            {
                RCLCPP_INFO(get_logger(), "成功移动到: %s", target_name.c_str());
                std::this_thread::sleep_for(std::chrono::milliseconds(500));
                return true;
            }
            else
            {
                RCLCPP_ERROR(get_logger(), "执行移动到 %s 失败", target_name.c_str());
                return false;
            }
        }
        else
        {
            RCLCPP_ERROR(get_logger(), "规划移动到 %s 失败", target_name.c_str());
            return false;
        }
    }

    void handlePostPathSequence()
    {
        RCLCPP_INFO(get_logger(), "开始视觉……");
        members_.visual_control_node = std::make_shared<VisualControl>(/* 可传入必要参数 */);
        members_.visual_control_node->initialize();
        std::thread visual_thread([this]()
        {
            while(rclcpp::ok() && !members_.visual_control_node->isFinished()){
                rclcpp::spin_some(members_.visual_control_node);
                std::this_thread::sleep_for(std::chrono::milliseconds(100));
            }
        });
        visual_thread.join();  // 等待视觉节点执行完成
        RCLCPP_INFO(get_logger(), "视觉节点任务完成，返回主逻辑");
        members_.is_executing = false;
    }

    bool returnToMiddle()
    {
        return moveToNamedTarget("middle");
    }

    bool continueCartesianPath(const std::vector<geometry_msgs::msg::Pose> &original_waypoints)
    {
        geometry_msgs::msg::Pose current_pose = members_.move_group->getCurrentPose().pose;
        std::vector<geometry_msgs::msg::Pose> remaining_waypoints;
        double min_distance = std::numeric_limits<double>::max();

        for (size_t i = 0; i < original_waypoints.size(); ++i)
        {
            double dx = current_pose.position.x - original_waypoints[i].position.x;
            double dy = current_pose.position.y - original_waypoints[i].position.y;
            double dz = current_pose.position.z - original_waypoints[i].position.z;
            double distance = std::sqrt(dx * dx + dy * dy + dz * dz);
            if (distance < min_distance)
            {
                min_distance = distance;
                members_.closest_index = i;
            }
        }

        for (size_t i = members_.closest_index + 1; i < original_waypoints.size(); ++i)
        {
            remaining_waypoints.push_back(original_waypoints[i]);
        }

        if (remaining_waypoints.empty())
            return false;

        members_.move_group->setStartStateToCurrentState();
        moveit::planning_interface::MoveGroupInterface::Plan new_plan;
        double fraction = members_.move_group->computeCartesianPath(remaining_waypoints, 0.01, 0.0, new_plan.trajectory_);
        if (fraction >= 0.9)
        {
            members_.move_group->asyncExecute(new_plan);
            return true;
        }
        return false;
    }

    void executeCartesianPath3()
    {
        if (!returnToMiddle())
            return;
        auto waypoints = path_plan::getTrajectoryPoints();
        members_.move_group->stop();
        members_.move_group->setStartStateToCurrentState();
        moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
        double fraction = members_.move_group->computeCartesianPath(waypoints, 0.01, 0.0, cartesian_plan.trajectory_);
        if (fraction < 0.9)
        {
            RCLCPP_ERROR(get_logger(), "初始路径规划失败，完成度: %f", fraction);
            return;
        }
        RCLCPP_INFO(get_logger(), "初始路径规划成功，开始执行...");
        members_.move_group->asyncExecute(cartesian_plan);
        bool is_stopped = false;
        bool is_forced = false;
        std::atomic<bool> motion_complete(false);
        auto start_time = std::chrono::steady_clock::now();
        const std::chrono::seconds timeout(15);
        while (rclcpp::ok() && !motion_complete && !is_forced)
        {
            auto elapsed_time = std::chrono::steady_clock::now() - start_time;
            if (elapsed_time > timeout)
            {
                RCLCPP_ERROR(get_logger(), "运动执行超时");
                motion_complete = true;
                break;
            }
            if (!members_.force_sign.load())
            {
                if (!is_stopped)
                {
                    RCLCPP_INFO(get_logger(), "检测到过大力，尝试停止运动...");
                    members_.move_group->stop();
                    is_stopped = true;
                    is_forced = true;
                    RCLCPP_INFO(get_logger(), "运动已停止");
                }
            }
            else
            {
                if (is_stopped)
                {
                    RCLCPP_INFO(get_logger(), "力信号恢复，尝试继续运动...");
                    if (continueCartesianPath(waypoints))
                    {
                        RCLCPP_INFO(get_logger(), "已恢复运动");
                        is_stopped = false;
                    }
                    else
                    {
                        RCLCPP_INFO(get_logger(), "无法继续运动或路径已完成");
                        motion_complete = true;
                        break;
                    }
                }
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
        }
        if (motion_complete.load())
            RCLCPP_INFO(get_logger(), "运动执行循环结束（可能已完成或无法继续）");
        else if (!rclcpp::ok())
            RCLCPP_INFO(get_logger(), "运动执行循环因 rclcpp 关闭而结束");
        members_.move_group->stop();
        RCLCPP_INFO(get_logger(), "executeCartesianPath3 结束");
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<CombinedNode>();

    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);

    RCLCPP_INFO(node->get_logger(), "节点已初始化，等待 result2_topic 消息...");
    RCLCPP_INFO(node->get_logger(), "开始执行器 spin");
    executor.spin();

    RCLCPP_INFO(node->get_logger(), "执行器 spin 结束");
    rclcpp::shutdown();
    return 0;
}